#include "includes.h"

extern int cnt[2];
extern int motor_t;

extern int trace[AMP_HISTORY];

/* TPM0_CH0: left_forward
 * TPM0_CH1: left_backward
 * TPM0_CH2: right_backward
 * TPM0_CH3: right_forward
 */
control_struct motor[2],anti[2];
extern control_struct servo;
extern track_status gramb_track;
extern led_status led[4];

double duty_out[2];
const double anti_k=0.0014444;

void motor_duty_limit(void){
    int i;
    for(i=0; i<2; ++i){
        motor[i].duty_out=MIN(motor[i].duty_out, MOTOR_DUTY_MAX);
        motor[i].duty_out=MAX(motor[i].duty_out, MOTOR_DUTY_MIN);
    }
}

void duty_limit(void){
    int i;
    for(i=0; i<2; ++i){
        duty_out[i]=MIN(duty_out[i], MOTOR_DUTY_MAX);
        duty_out[i]=MAX(duty_out[i], -MOTOR_ANTI_MIN);
    }
}

void motor_set_limit(void){
    int i;
    for(i=0; i<2; ++i){
        motor[i].u=MIN(motor[i].u, MOTOR_SET_MAX);
        motor[i].u=MAX(motor[i].u, MOTOR_SET_MIN);
    }
}

void motor_duty_set(void){
    int i;
    /*
    const double a=-2.5,k=MOTOR_SET_MAX;
    */
    
    if(gramb_track.instraight){
        for(i=0; i<2; ++i)
            motor[i].u=MOTOR_DUTY_STRAIGHT;
    }else{
        for(i=0; i<2; ++i)
            motor[i].u=MOTOR_DUTY_STEERING;
            /*
            motor[i].u=a*servo.e[servo.t]*servo.e[servo.t]+k;
            */
    }

    motor_set_limit();
}

/*
void electronic_differential_lock(void){
    if(gramb_track.instraight)
        return;
    
    if(gramb_track.servo_turn>0){
        motor[1].duty_out=motor[1].duty_out;
        motor[0].duty_out=motor[1].duty_out;
    }

    if(gramb_track.servo_turn<0){
        motor[0].duty_out=motor[0].duty_out;
        motor[1].duty_out=motor[0].duty_out;
    }

    motor_duty_limit();
}
*/

/* direct control */
void motor_direct(void){
    int i;
    for(i=0; i<2; ++i)
        motor[i].duty_out=motor[i].u;

    motor_duty_limit();
}

/* bang bang control */
void motor_bang_bang(void){
    int i;
    if(gramb_track.instraight){
        for(i=0; i<2; ++i)
            motor[i].duty_out=MOTOR_DUTY_STRAIGHT;
    }else{
        for(i=0; i<2; ++i)
            motor[i].duty_out=MOTOR_DUTY_STEERING;
    }
}

/* pid control */
void motor_error_calc(void){
    /* estimate expected motor speed */
    motor[0].s=0.02087*motor[0].u-1.932;
    motor[1].s=0.02003*motor[1].u+1.888;

    /* calculate speed error */
    motor[0].e[motor[0].t]=motor[0].s-motor[0].n;
    motor[1].e[motor[1].t]=motor[1].s-motor[1].n;
}

double motor_pid_k[3]={
    20.0,
    1.0,
    0.0
};

void motor_pid(void){
    int i;

    motor_error_calc();
    for(i=0; i<2; ++i)
        duty_out[i]+=motor[i].k[0]*(motor[i].e[motor[i].t]-motor[i].e[IN_RANGE(motor[i].t-1, MOTOR_PID_T)])+motor[i].k[1]*motor[i].e[motor[i].t]+motor[i].k[2]*(motor[i].e[motor[i].t]-2*motor[i].e[IN_RANGE(motor[i].t-1, MOTOR_PID_T)]+motor[i].e[IN_RANGE(motor[i].t-2, MOTOR_PID_T)]);

    duty_limit();
    for(i=0; i<2; ++i)
        if(duty_out[i]>0){
            motor[i].duty_out=duty_out[i];
            anti[i].duty_out=0;
        }else{
            motor[i].duty_out=0;
            anti[i].duty_out=-duty_out[i]*(motor[i].n*motor[i].n*anti_k);
        }

    motor_duty_limit();
}

void motor_control(void){
    /* control motor for every 5ms */
    if(motor_t!=0)
        return;

    motor[0].n=cnt[0];
    motor[1].n=cnt[1];
    
    motor_duty_set();
    /*
    electronic_differential_lock();
    */
    
#if MOTOR_CONTROL==0
    motor_direct();
#elif MOTOR_CONTROL==1
    motor_bang_bang();
#elif MOTOR_CONTROL==2
    motor_pid();
#endif
}
